#include "tank_power.h"
#include "cmsis_gcc.h"
#include "stm32f1xx_hal_conf.h"

extern float INIT_CAMERA_SERVO_1;
extern float INIT_CAMERA_SERVO_2;
extern float INIT_CAMERA_SERVO_1_MAX;
extern float INIT_CAMERA_SERVO_2_MAX;
extern float INIT_CAMERA_SERVO_1_MIN;
extern float INIT_CAMERA_SERVO_2_MIN;

extern float g_settingXServoIncreaseValue;
extern float g_settingYServoIncreaseValue;

static float nowServoX = 0.0f;
static float nowServoY = 0.0f;

/**
 * @brief 单独复位舵机
 */
void resetServo()
{
    TIM3->CCR1 = TIM3->ARR * INIT_CAMERA_SERVO_1 / 1000;
    TIM3->CCR2 = TIM3->ARR * INIT_CAMERA_SERVO_2 / 1000;
    nowServoX  = INIT_CAMERA_SERVO_1;
    nowServoY  = INIT_CAMERA_SERVO_2;
}

void resetPower()
{
    TIM8->CCR1 = 0;
    TIM8->CCR2 = 0;
    TIM8->CCR3 = 0;
    TIM8->CCR4 = 0;
}

/**
 * @brief 复位
 */
void reset()
{
    resetServo();

    TIM8->CCR1 = 0;
    TIM8->CCR2 = 0;
    TIM8->CCR3 = 0;
    TIM8->CCR4 = 0;
    //   __ASM volatile ("smlsldx %0, %1, %2, %3" : "=r" (llr.w32[1]), "=r"
    //   (llr.w32[0]): "r" (op1), "r" (op2) , "0" (llr.w32[1]), "1" (llr.w32[0])
    //   );
}

/**
 * @brief 初始位置
 */
void init()
{
    reset();

    HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_2);

    HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_2);
    HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_3);
    HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_4);
}

/**
 * @brief Set full Motor Power
 * @param left_forward_: 左履带前进速度比率
 * @param left_backword_: 左履带后退速度比率
 * @param right_forward_: 右履带前进速度比率
 * @param right_backword_: 右履带后退速度比率
 */
static void setMotorPower(const float left_forward_,
                          const float left_backword_,
                          const float right_forward_,
                          const float right_backword_)
{
    assert_param(left_forward_ > 0);
    assert_param(left_backward_ > 0);
    assert_param(right_forward_ > 0);
    assert_param(right_backward_ > 0);
    TIM8->CCR1 = TIM8->ARR * left_backword_;
    TIM8->CCR2 = TIM8->ARR * left_forward_;

    TIM8->CCR3 = TIM8->ARR * right_backword_;
    TIM8->CCR4 = TIM8->ARR * right_forward_;
}

static void setServoValue(const float x_, const float y_)
{
    assert_param(x_ > 0);
    assert_param(y_ > 0);
    TIM3->CCR1 = TIM3->ARR * x_ / 1000;
    TIM3->CCR2 = TIM3->ARR * y_ / 1000;

    nowServoX = x_;
    nowServoY = y_;
}

/**
 * @brief Set Power
 * @param left_: 左边履带输出功率比
 * @param right_: 右边履带输出功率比
 */
void setPower(const float left_, const float right_)
{
    float left_forward = 0, left_backword = 0, right_forward = 0,
          right_backword = 0;

    if (left_ < 0) {
        left_backword = -left_;
    }
    else {
        left_forward = left_;
    }

    if (right_ < 0) {
        right_backword = -right_;
    }
    else {
        right_forward = right_;
    }

    setMotorPower(left_forward, left_backword, right_forward, right_backword);
}

/**
 * @brief 刹车
 */
void brake()
{
    setMotorPower(1, 1, 1, 1);
}

/**
 * @brief 控制两个舵机
 * @param offsetX: 偏移量
 * @param offsetY: 偏移量
 */
void setServo(const float offsetX, const float offsetY)
{
    float xValue = INIT_CAMERA_SERVO_1;
    float yValue = INIT_CAMERA_SERVO_2;
    if (offsetX < 0) {
        xValue = INIT_CAMERA_SERVO_1 +
                 ((INIT_CAMERA_SERVO_1 - INIT_CAMERA_SERVO_1_MIN) * offsetX);
    }
    else if (offsetX > 0) {
        xValue = INIT_CAMERA_SERVO_1 +
                 ((INIT_CAMERA_SERVO_1_MAX - INIT_CAMERA_SERVO_1) * offsetX);
    }
    else {
        xValue = INIT_CAMERA_SERVO_1;
    }

    if (offsetY < 0) {
        yValue = INIT_CAMERA_SERVO_2 +
                 ((INIT_CAMERA_SERVO_2_MAX - INIT_CAMERA_SERVO_2) * (-offsetY));
    }
    else if (offsetY > 0) {
        yValue = INIT_CAMERA_SERVO_2 -
                 ((INIT_CAMERA_SERVO_2 - INIT_CAMERA_SERVO_2_MIN) * (offsetY));
    }
    else {
        yValue = INIT_CAMERA_SERVO_2;
    }

    setServoValue(xValue, yValue);
}

int increaseServo(const float offsetX, const float offsetY)
{
    do {
        nowServoX += offsetX;
        if (nowServoX > INIT_CAMERA_SERVO_1_MAX ||
            nowServoX < INIT_CAMERA_SERVO_1_MIN) {
            break;
        }
        nowServoY += offsetY;
        if (nowServoY > INIT_CAMERA_SERVO_1_MAX ||
            nowServoY < INIT_CAMERA_SERVO_1_MIN) {
            break;
        }
        setServoValue(nowServoX, nowServoY);

        return 0;
    } while (0);
    return -1;
}

/**
 * @brief 小步长增加/减少x轴
 * @param direct: 0 为增加
 * @return int: is max value
 */
int miniIncreaseServoX(uint8_t direct)
{
    do {
        float res = nowServoX;
        if (direct == 0) {
            res += g_settingXServoIncreaseValue;
        }
        else {
            res -= g_settingXServoIncreaseValue;
        }
        if (res > INIT_CAMERA_SERVO_1_MAX || res < INIT_CAMERA_SERVO_1_MIN) {
            break;
        }
        nowServoX = res;
        setServoValue(nowServoX, nowServoY);

        return 0;
    } while (0);
    return -1;
}

/**
 * @brief 小步长增加/减少y轴
 * @param direct: 0 为增加
 * @return int: is max value
 */
int miniIncreaseServoY(uint8_t direct)
{
    do {
        float res = nowServoY;
        if (direct == 0) {
            res += g_settingYServoIncreaseValue;
        }
        else {
            res -= g_settingYServoIncreaseValue;
        }
        if (res > INIT_CAMERA_SERVO_2_MAX || res < INIT_CAMERA_SERVO_2_MIN) {
            break;
        }
        nowServoY = res;
        setServoValue(nowServoX, nowServoY);

        return 0;
    } while (0);
    return -1;
}

/**
 * @brief 车辆旋转
 * @param direct: 0为顺时针
 * @param power: 必须为正
 */
void rotatePower(const uint8_t direct, const float power)
{
    if (direct == 0) {
        setPower(power, -power);
    }
    else {
        setPower(-power, power);
    }
}
